System and method of registration between devices with movable arms

ABSTRACT

A system and method of registration between devices includes a tracking system and a controller. The system is configured to couple to a first device and a second arm separate from the first device. The first device has a first arm configured to couple to a manipulatable device. The second arm is configured to couple to an imaging device. The controller is configured to determine, based on tracking data from the tracking system, a first pose of the first arm and a second pose of the second arm; send a movement command to the second arm that directs the second arm to move into a third pose while maintaining a safety margin between the first and second arms. In the third pose the imaging device can capture an image of at least a portion of the manipulatable device. The movement command is based on the first and second poses.

CROSS-REFERENCE TO RELATED APPLICATIONS

This application is a continuation of U.S. patent application Ser. No.14/862,692 filed Sep. 23, 2015, which is a continuation of U.S. Pat. No.9,259,282 filed Dec. 10, 2013, and claims priority to U.S. ProvisionalApplication No. 61/735,170 filed Dec. 10, 2012 and entitled “CollisionAvoidance During Controlled Movement of Image Capturing Device andManipulatable Robot Arms”, each of which is incorporated herein in theirentirety by reference.

FIELD OF THE INVENTION

The present invention generally relates to robotic systems and inparticular, to collision avoidance during controlled movement of imagecapturing device and manipulatable device robot arms.

BACKGROUND OF THE INVENTION

Robotic systems and computer-assisted devices often include robot ormovable arms to manipulate instruments for performing a task at a worksite and at least one robot or movable arm for supporting an imagecapturing device which captures images of the work site. When the robotarms are operating within close proximity to each other, there is apossibility that the arms may collide with one another and by so doing,cause damage to the arms. When the robot arms are commonly operated,preventing such collisions may be straightforward. However, when therobot arms are independently operated, collision avoidance may be muchmore challenging.

A robot arm comprises a plurality of links coupled together by one ormore actively controlled joints. In many embodiments, a plurality ofactively controlled joints may be provided. The robot arm may alsoinclude one or more passive joints, which are not actively controlled,but comply with movement of an actively controlled joint. Such activeand passive joints may be revolute or prismatic joints. Theconfiguration of the robot arm may then be determined by the positionsof the joints and knowledge of the structure and coupling of the links.

To avoid collisions between robot arms it is useful to have informationof the configurations of the arms and information of their respectivepositions in a common reference frame. With this information, acontroller that is controlling the movement of one of the robot arms cantake action to avoid a collision with another robot arm.

One action that the controller may take to avoid collisions is to warnan operator who is commanding a robot arm that a collision with anotherrobot arm is imminent. As one example of such a warning system, U.S.2009/0192524 A1 entitled “Synthetic Representation of a Surgical Robot,”which is incorporated herein by reference, describes a patient side cartupon which a plurality of robot arms is mounted. As another example ofsuch a warning system, U.S. 2009/0326553 entitled “Medical RoboticSystem Providing an Auxiliary View of Articulatable InstrumentsExtending out of a Distal End of an Entry Guide,” which is incorporatedherein by reference, describes a medical robotic system having aplurality of articulated instruments extending out of an entry guide. Asyet another example of such a warning system, U.S. 2009/0234444 A1entitled “Method and Apparatus for Conducting an InterventionalProcedure Involving Heart Valves using a Robot-based X-ray Device,”which is incorporated herein by reference, describes a medical roboticsystem in which an X-ray source and detector are mounted on opposingends of a C-arm so that X-ray images of a patient's anatomy can becaptured during the performance of a medical procedure using a cathetercontrol robot.

Collision avoidance may also be performed automatically by a controllerwhich is controlling movement of one or more robot arms. As an exampleof such an automatic collision avoidance system, U.S. Pat. No. 8,004,229entitled “Software center and highly configurable robotic systems forsurgery and other uses,” which is incorporated herein by reference,describes a medical robotic system that is configured to avoidcollisions between its robot arms. The robot arms have redundancy sothat multiple configurations are possible for each arm to achieve adesired position and orientation of its held instrument. Each controllercommands movement of its associated robot arm subject to a secondaryconstraint that results in eliminating possible arm configurations thatwould result in a collision with another robot arm.

When a plurality of robot arms is controlled by a plurality ofcontrollers, however, confusion and unintended consequences may resultif more than one of the plurality of controllers is attempting to avoidcollisions. This problem is exacerbated when there is none or onlylimited communication of information between the controllers, such asmay be the case when the robotic system employs independently operatedrobot arms. To avoid robot arm collision problems, the independentlyoperated robot arms may be used sequentially, but not concurrently atthe work site. However, it may be advantageous to use the robot armsconcurrently during the performance of a procedure or task at a worksite.

SUMMARY OF THE INVENTION

Accordingly, one object of one or more aspects of the present inventionis a robotic system and method implemented therein that automaticallyperforms collision avoidance of robot arms.

Another object of one or more aspects of the present invention is arobotic system and method implemented therein that automaticallyperforms collision avoidance of independently operated robot arms.

Another object of one or more aspects of the present invention is arobotic system and method implemented therein that automaticallyperforms collision avoidance of robot arms without preventing any of therobot arms from performing its intended task.

These and additional objects are accomplished by the various aspects ofthe present invention, wherein briefly stated, one aspect is a roboticsystem including: a first robot arm holding a manipulatable devicehaving a working end; and a controller configured to control movement ofat least one of the first robot arm and a second robot arm whileavoiding a collision between the first and second robot arms, whereinthe second robot arm holds an image capturing device for capturing aplurality of images of the working end of the manipulatable device fromwhich a three-dimensional computer model of the working end of themanipulatable device is generatable; wherein the controller isconfigured to avoid the collision between the first and second robotarms: by determining a position and an orientation of the working end ofthe manipulatable device relative to a reference frame of the imagecapturing device by using at least one image of the plurality of imagesof the working end of the manipulatable device, wherein the referenceframe of the image capturing device corresponds to a perspective of thecaptured images; by determining a configuration and position of one ofthe first and second robot arms relative to a common reference frame byusing the determined position and orientation of the working end of themanipulatable device relative to the reference frame of the imagecapturing device; and by determining a configuration and position of theother of the first and second robot arms relative to the commonreference frame by using joint position information received for theother of the first and second robot arms; by determining an imminentcollision between the first and second robot arms using the determinedconfigurations and positions of the first and second robot arms relativeto the common reference frame; and by commanding an action to be takenby one of the first and second robot arms to avoid the imminentcollision.

Another aspect is a method implemented in a robotic system for avoidingcollisions between first and second robot arms, wherein the first robotarm holds a manipulatable device having a working end, wherein thesecond robot arm holds an image capturing device for capturing aplurality of images of the working end of the manipulatable device fromwhich a three-dimensional computer model of the working end of themanipulatable device is generatable, and wherein the method includes:determining a position and an orientation of the working end of themanipulatable device relative to a reference frame of the imagecapturing device by using at least one image of the plurality of imagesof the working end of the manipulatable device, wherein the referenceframe of the image capturing device corresponds to a perspective of thecaptured images; determining a configuration and position of one of thefirst and second robot arms relative to a common reference frame byusing the determined position and orientation of the working end of themanipulatable device relative to the reference frame of the imagecapturing device; determining a configuration and position of the otherof the first and second robot arms relative to the common referenceframe by using joint position information received for the other of thefirst and second robot arms; determining an imminent collision betweenthe first and second robot arms using the determined configurations andpositions of the first and second robot arms relative to the commonreference frame; and commanding an action to be taken by one of thefirst and second robot arms to avoid the imminent collision.

Another aspect is a robotic system including: a first robot arm that isholding a manipulatable device having a working end; and a processorprogrammed to register the first robot arm to a second robot arm that isholding an image capturing device, wherein the processor performs theregistration by: performing a low accuracy registration of the first andsecond robot arms at initial positions of the first and second robotarms using external tracking data and kinematics data of at least one ofthe first and second robot arms; performing a mid accuracy registrationof the first and second robot arms relative to a first setup positionusing at least one image captured by the image capturing device of atleast a part of the first robot arm and kinematics data of at least oneof the first and second robot arms, wherein the first setup positionincludes a first safety margin indicative of the low accuracyregistration; and performing a high accuracy registration of the firstand second robot arms relative to a second setup position using at leastone image captured by the image capturing device of the working end ofthe manipulatable device and kinematics data of at least one of thefirst and second robot arms, wherein the second setup position includesa second safety margin indicative of the mid accuracy registration.

Another aspect is a method implemented by a processor for registeringfirst and second robot arms. The first robot arm holds a manipulatabledevice having a working end. The second robot arm holds an imagecapturing device. The method includes performing a low accuracyregistration of the first and second robot arms at initial positions ofthe first and second robot arms using external tracking data andkinematics data of at least one of the first and second robot arms;performing a mid accuracy registration of the first and second robotarms relative to a first setup position using at least one imagecaptured by the image capturing device of at least a part of the firstrobot arm and kinematics data of at least one of the first and secondrobot arms, wherein the first setup position includes a first safetymargin indicative of the low accuracy registration; and performing ahigh accuracy registration of the first and second robot arms relativeto a second setup position using at least one image captured by theimage capturing device of the working end of the manipulatable deviceand kinematics data of at least one of the first and second robot arms,wherein the second setup position includes a second safety marginindicative of the mid accuracy registration.

Consistent with some embodiments, a movement control system includes acontroller. The controller includes one or more processors and memorycoupled to the one or more processors. The controller is coupled to acomputer-assisted surgical device having a first movable arm coupled toa manipulatable device having a working end. The controller is furthercoupled to a second movable arm coupled to an image capturing deviceseparate from the computer-assisted surgical device. The controller isconfigured to receive one or more first configurations for the firstmovable arm; receive one or more second configurations for the secondmovable arm; receive a first plurality of images of the working end fromthe image capturing device; determine, based on at least one of thefirst plurality of images, a position and an orientation of the workingend in a common reference frame; determine, based on the firstconfigurations, a first movable arm position and a first movable armtrajectory for the first movable arm in the common reference frame;determine, based on the second configurations, a second movable armposition and a second movable arm trajectory for the second movable armin the common reference frame; based on the first movable arm position,the first movable arm trajectory, the second movable arm position, andthe second movable arm trajectory, determine whether motion of the firstmovable arm, motion of the second movable arm, or motions of the firstand second movable arms together will result in an undesirablerelationship between the first and second movable arms; and send a firstmovement command to the first movable arm or the second movable arm toavoid the undesirable relationship.

In some examples, the common reference frame is a reference frame of theimage capturing device and the controller is further configured todetermine the first movable arm position in the common reference framefurther based on the position and the orientation of the working end inthe common reference frame. In some examples, the common reference frameis a reference frame of the computer-assisted surgical device and thecontroller is further configured to determine the second movable armposition in a reference frame of the image capturing device, determinethe position and the orientation of the working end in the referenceframe of the image capturing device, and transform the second movablearm position and the position and orientation of the working end fromthe reference frame of the image capturing device to the commonreference frame.

In some examples, the first plurality of images are a plurality oftwo-dimensional images from a image capturing device and the controlleris further configured to determine the position and the orientation ofthe working end from the two-dimensional images even when the workingend of the manipulatable device is occluded by one or more objectsdisposed between the image capturing device and the working end of themanipulatable device. In some examples, the system further includes aviewer adapted to display a second plurality images of a work space ofthe working end of the manipulatable device and an input unit configuredto receive information of a user specified region of interest within theimages being displayed on the viewer. The controller is furtherconfigured to send second movement commands to the second movable arm sothat the image capturing device captures images of the user specifiedregion of interest.

In some examples, the input unit includes a gaze tracking unitconfigured to track a gaze of a user on a display screen of the viewerand the gaze tracking unit includes an indicator operable by the user toindicate the region of interest by using a current gaze point of theuser on the display screen. In some examples, the input unit includes atelestrator unit configured to receive one of the second plurality ofimages and display the received one of the second plurality of images ona display screen of the telestrator and the telestrator unit includes amarker unit operable by a user to indicate the region of interest on thedisplayed one of the second plurality of images.

In some examples, the system further includes a camera, and the secondplurality of images are captured by the camera. In some examples, systemfurther includes an ultrasound probe, and the second plurality of imagesare captured by the ultrasound probe. In some examples, the firstmovable arm has redundant degrees of freedom so that for eachcontrollable position and orientation of the working end of themanipulatable device there are a first plurality of possible positionsand orientations for the first movable arm, the second movable arm hasredundant degrees of freedom so that for each controllable position andorientation of the image capturing device there are a second pluralityof possible positions and orientations for the second movable arm, andthe first movement command sent to the first movable arm or the secondmovable arm directs the first movable arm to move to one of the firstplurality of possible positions and orientations or directs the secondmovable arm to move to one of the second plurality of possible positionsand orientations that avoid the undesirable relationship.

In some examples, the controller is further configured to determinewhich one of the first movable arm and the second movable arms is to besent the first movement command based on differences between the firstmovable arm position and the first plurality of possible positions andorientations and differences between the second movable arm position andthe second plurality of possible positions and orientations and thedetermination is made so as to minimize a cost function.

In some examples, the first movement command directs the first movablearm to move from the first movable arm position to a first selected oneof the first plurality of possible positions and orientations or directsthe second movable arm to move from the second movable arm position to asecond selected one of the second plurality of possible positions andorientations based on which of the first selected one of the firstplurality of possible positions and orientations and the second selectedone of the second plurality of possible positions and orientationsminimizes the cost function. In some examples, the undesirablerelationship is selected from a group consisting of a collision betweenthe first movable arm and the second movable arm, too close a proximitybetween the first movable arm and the second movable arm, andobstruction of a region of interest of the image capturing device by thefirst movable arm.

Consistent with some embodiments, a method of controlling movement in amedical system includes receiving one or more first configurations for afirst movable arm of a computer-assisted surgical device, the firstmovable arm being coupled to a manipulatable device having a workingend; receiving one or more second configurations for a second movablearm coupled to an image capturing device separate from thecomputer-assisted surgical device; receiving a first plurality of imagesof the working end from the image capturing device; determining, basedon at least one of the first plurality of images, a position and anorientation of the working end in a common reference frame; determining,based on the first configurations, a first movable arm position and afirst movable arm trajectory for the first movable arm in the commonreference frame; determining, based on the second configurations, asecond movable arm position and a second movable arm trajectory for thesecond movable arm in the common reference frame; based on the firstmovable arm position, the first movable arm trajectory, the secondmovable arm position, and the second movable arm trajectory, determiningwhether motion of the first movable arm, motion of the second movablearm, or motions of the first and second movable arms together willresult in an undesirable relationship between the first and secondmovable arms; and sending a first movement command to the first movablearm or the second movable arm to avoid the undesirable relationship.

In some examples, the common reference frame is a reference frame of theimage capturing device, and the method further includes determining thefirst movable arm position in the common reference frame further basedon the position and the orientation of the working end in the commonreference frame. In some examples, the common reference frame is areference frame of the computer-assisted surgical device and the methodfurther includes determining the second movable arm position in areference frame of the image capturing device, determining the positionand the orientation of the working end in the reference frame of theimage capturing device, and transforming the second movable arm positionand the position and orientation of the working end from the referenceframe of the image capturing device to the common reference frame.

In some examples, the first plurality of images are a plurality oftwo-dimensional images from a image capturing device and the methodfurther includes determining the position and the orientation of theworking end from the two-dimensional images even when the working end ofthe manipulatable device is occluded by one or more objects disposedbetween the image capturing device and the working end of themanipulatable device. In some examples, the method further includesdetermining the first movement command so as to minimize a costfunction.

Consistent with some embodiments, a movement control system includes atracking system and a controller coupled to the tracking system. Thecontroller includes one or more processors and memory coupled to the oneor more processors. The controller is coupled to a computer-assistedsurgical device having a first movable arm coupled to a manipulatabledevice having a working end. The controller is further coupled to asecond movable arm coupled to an image capturing device separate fromthe computer-assisted surgical device. The controller is configured toreceive first kinematic data for the first movable arm; receive secondkinematic data for the second movable arm; receive first tracking datafor the first movable arm from the tracking system; receive secondtracking data for the second movable arm from the tracking system;determine, based on the first kinematic data and the first trackingdata, a first location of the first movable arm; determine, based on thesecond kinematic data and the second tracking data, a second location ofthe second movable arm; send a first movement command to the secondmovable arm that directs the second movable arm to move into a firstset-up position to capture images of at least a portion of themanipulatable device while maintaining a first safety margin between thefirst movable arm and the second movable arm, the first movement commandbeing based on the first location and the second location; receive thirdkinematic data for the first movable arm; receive fourth kinematic datafor the second movable arm; receive one or more first images from theimage capturing device, the one or more first images capturing at leasta portion of the manipulatable device; determine, based on the thirdkinematic data and the one or more first images, a third location of thefirst movable arm; determine, based on the fourth kinematic data and theone or more first images, a fourth location of the second movable arm;and send a second movement command to the second movable arm thatdirects the second movable arm to move into a second set-up position,different from the first set-up position, to capture images of a regionof interest while maintaining a second safety margin between the firstmovable arm and the second movable arm, the second movement commandbeing based on the third location and the fourth location.

In some examples, the controller is further configured to receive fifthkinematic data for the first movable arm; receive sixth kinematic datafor the second movable arm; receive one or more second images from theimage capturing device, the one or more second images capturing at leasta portion of the working end; determine, based on the fifth kinematicdata and the one or more second images, a fifth location of the firstmovable arm; determine, based on the sixth kinematic data and the one ormore second images, a sixth location of the second movable arm; and senda third movement command to the second movable arm that directs thesecond movable arm to move into a third set-up position, different fromthe first set-up position and the second set-up position, to captureimages of the working end while maintaining a third safety marginbetween the first movable arm and the second movable arm, the thirdmovement command being based on the fifth location and the sixthlocation.

In some examples, the second safety margin is an order of magnitudesmaller than the first safety margin. In some examples, maintaining thefirst safety margin keeps a separation between the first movable arm andthe second movable arm of at least ten centimeters and maintaining thesecond safety margin keeps a separation between the first movable armand the second movable arm of at least one centimeter. In some examples,the third safety margin is an order of magnitude smaller than the secondsafety margin. In some examples, maintaining the second safety marginkeeps a separation between the first movable arm and the second movablearm of at least one centimeter and maintaining the third safety marginkeeps a separation between the first movable arm and the second movablearm of at least one millimeter.

In some examples, the first images are two-dimensional images from aimage capturing device and the controller is further configured todetermine a position and an orientation of the manipulatable device fromthe two-dimensional images even when the manipulatable device isoccluded by one or more objects disposed between the image capturingdevice and the working end of the manipulatable device. In someexamples, the second images are two-dimensional images from a imagecapturing device and the controller is further configured to determine aposition and an orientation of the working end from the two-dimensionalimages even when the working end of the manipulatable device is occludedby one or more objects disposed between the image capturing device andthe working end of the manipulatable device. In some examples, thesystem further includes a viewer adapted to display one more secondimages received from the image capturing device and an input unitconfigured to receive information of the region of interest within thesecond images.

In some examples, the input unit includes a gaze tracking unitconfigured to track a gaze of a user on a display screen of the viewerand the gaze tracking unit includes an indicator operable by the user toindicate the region of interest by using a current gaze point of theuser on the display screen. In some examples, the input unit includes atelestrator unit configured to receive one of the second of images anddisplay the received one of the second images on a display screen of thetelestrator and the telestrator unit includes a marker unit operable bya user to indicate the region of interest on the displayed one of thesecond images. In some examples, the system further includes a cameraand the second images are captured by the camera. In some examples, thesystem further includes an ultrasound probe and the second images arecaptured by the ultrasound probe. In some examples, the first movementcommand minimizes a cost function.

In some examples, the second movable arm has redundant degrees offreedom so that for each controllable position and orientation of theimage capturing device there are a plurality of possible positions andorientations for the second movable arm, the first movement command sentto the second movable arm directs the second movable arm to move to oneof the plurality of possible positions and orientations that maintainthe first safety margin, the second movement command sent to the secondmovable arm directs the second movable arm to move to one of theplurality of possible positions and orientations that maintain thesecond safety margin, and the third movement command sent to the secondmovable arm directs the second movable arm to move to one of theplurality of possible positions and orientations that maintain the thirdsafety margin.

Consistent with some embodiments, a method of controlling movement in amedical system includes receiving first kinematic data for a firstmovable arm of a computer-assisted surgical device, the first movablearm being coupled to a manipulatable device having a working end;receiving second kinematic data for a second movable arm coupled to animage capturing device separate from the computer-assisted surgicaldevice; receiving first tracking data for the first movable arm from atracking system; receiving second tracking data for the second movablearm from the tracking system; determining, based on the first kinematicdata and the first tracking data, a first location of the first movablearm; determining, based on the second kinematic data and the secondtracking data, a second location of the second movable arm; sending afirst movement command to the second movable arm that directs the secondmovable arm to move into a first set-up position to capture images of atleast a portion of the manipulatable device while maintaining a firstsafety margin between the first movable arm and the second movable arm,the first movement command being based on the first location and thesecond location; receiving third kinematic data for the first movablearm; receiving fourth kinematic data for the second movable arm;receiving one or more first images from the image capturing device, theone or more first images capturing at least a portion of themanipulatable device; determining, based on the third kinematic data andthe one or more first images, a third location of the first movable arm;determining, based on the fourth kinematic data and the one or morefirst images, a fourth location of the second movable arm; and sending asecond movement command to the second movable arm that directs thesecond movable arm to move into a second set-up position, different fromthe first set-up position, to capture images of a region of interestwhile maintaining a second safety margin between the first movable armand the second movable arm, the second movement command being based onthe third location and the fourth location.

In some examples, the method further includes receiving fifth kinematicdata for the first movable arm; receiving sixth kinematic data for thesecond movable arm; receiving one or more second images from the imagecapturing device, the one or more second images capturing at least aportion of the working end; determining, based on the fifth kinematicdata and the one or more second images, a fifth location of the firstmovable arm; determining, based on the sixth kinematic data and the oneor more second images, a sixth location of the second movable arm; andsending a third movement command to the second movable arm that directsthe second movable arm to move into a third set-up position, differentfrom the first set-up position and the second set-up position, tocapture images of the working end while maintaining a third safetymargin between the first movable arm and the second movable arm, thethird movement command being based on the fifth location and the sixthlocation.

In some examples, the method further includes determining the thirdmovement command so as to minimize a cost function. In some examples,the first images are two-dimensional images from a image capturingdevice and the method further includes determining a position and anorientation of the manipulatable device from the two-dimensional imageseven when the manipulatable device is occluded by one or more objectsdisposed between the image capturing device and the working end of themanipulatable device. In some examples, the second images aretwo-dimensional images from a image capturing device and the methodfurther includes determining a position and an orientation of theworking end from the two-dimensional images even when the working end ofthe manipulatable device is occluded by one or more objects disposedbetween the image capturing device and the working end of themanipulatable device.

Consistent with some embodiments, a robotic system includes a firstrobot arm holding a manipulatable device having a working end and acontroller configured to control movement of at least one of the firstrobot arm and a second robot arm while avoiding a collision between thefirst and second robot arms. The second robot arm holds an imagecapturing device for capturing a plurality of images of the working endof the manipulatable device from which a three-dimensional computermodel of the working end of the manipulatable device is generatable. Thecontroller is configured to avoid the collision between the first andsecond robot arms by determining a position and an orientation of theworking end of the manipulatable device relative to a reference frame ofthe image capturing device by using at least one image of the pluralityof images of the working end of the manipulatable device, wherein thereference frame of the image capturing device corresponds to aperspective of the captured images; by determining a configuration andposition of one of the first and second robot arms relative to a commonreference frame by using the determined position and orientation of theworking end of the manipulatable device relative to the reference frameof the image capturing device; and by determining a configuration andposition of the other of the first and second robot arms relative to thecommon reference frame by using joint position information received forthe other of the first and second robot arms; by determining an imminentcollision between the first and second robot arms using the determinedconfigurations and positions of the first and second robot arms relativeto the common reference frame; and by commanding an action to be takenby one of the first and second robot arms to avoid the imminentcollision.

In some examples, the image capturing device includes a tomographicimage capturing device for capturing a plurality of two-dimensionalimages from which the working end of the manipulatable device isdiscernible even when the working end of the manipulatable device isoccluded by one or more objects disposed between the image capturingdevice and the working end of the manipulatable device. In someexamples, the controller is configured to determine the configurationand position of the first robot arm relative to the reference frame ofthe image capturing device by using information of the construction andgeometry of the first robot arm and the determined position andorientation of the working end of the manipulatable device relative tothe reference frame of the image capturing device.

In some examples, the controller is configured to determine theconfiguration and position of the second robot arm relative to areference frame of the manipulatable device by using a transform totranslate points in the reference frame of the image capturing device tothe reference frame of the manipulatable device. The transform has beendetermined by using the determined position and orientation of theworking end of the manipulatable device relative to the reference frameof the image capturing device. The reference frame of the manipulatabledevice is the common reference frame. In some examples, the systemfurther includes a viewer adapted to display images derived fromcaptured images of a work space of the working end of the manipulatabledevice and an input unit configured to receive information of a userspecified region of interest within the images being displayed on theviewer. The controller is configured to command and control movement ofthe second robot arm so that the image capturing device captures imagesof the user specified region of interest in the plurality of images ofthe working end of the manipulatable device.

In some examples, the input unit includes a gaze tracking unitconfigured to track a gaze of a user on a display screen of the viewer.The gaze tracking unit includes an indicator operable by the user toindicate the region of interest by using a current gaze point of theuser on the display screen. In some examples, the input unit includes atelestrator unit configured to receive an image being displayed on theviewer and display the received image on a display screen of thetelestrator. The telestrator unit includes a marker unit operable by auser to indicate the region of interest on the image being displayed onthe display screen of the telestrator unit.

In some examples, the system further includes a camera and the capturedimages of the work space of the working end of the manipulatable deviceare captured by the camera. In some examples, the system furtherincludes an ultrasound probe and the captured images of the work spaceof the working end of the manipulatable device are captured by theultrasound probe. In some examples, the controller is associated withthe first robot arm to control movement of the first robot arm whileavoiding a collision with the second robot arm and a reference frame ofthe manipulatable device is the common reference frame. In someexamples, the controller is associated with the second robot arm tocontrol movement of the second robot arm while avoiding a collision withthe first robot arm, and the reference frame of the image capturingdevice is the common reference frame.

In some examples, the first robot arm includes a first plurality ofjoints and a first plurality of links that are coupled together toprovide redundant degrees of freedom movement of the first robot arm sothat for each controllable position and orientation of the working endof the manipulatable device there are a plurality of possibleconfigurations for the first robot arm. The controller is configured tocontrol movement of the first robot arm by commanding the first robotarm to be configured in one of the plurality of possible configurationsfor the first robot arm according to a desired position and orientationof the working end of the manipulatable device and according to asecondary constraint for avoiding a collision between the first andsecond robot arms.

In some examples, the second robot arm includes a second plurality ofjoints and a second plurality of links that are coupled together toprovide redundant degrees of freedom movement of the second robot arm sothat for each controllable position and orientation of the imagecapturing device there are a plurality of possible configurations forthe second robot arm. The controller is configured to control movementof the second robot arm by commanding the second robot arm to beconfigured in one of the plurality of possible configurations for thesecond robot arm according to a desired position and orientation of theimage capturing device and according to a secondary constraint foravoiding a collision between the first and second robot arms.

In some examples, the first robot arm includes a first plurality ofjoints and a first plurality of links that are coupled together toprovide redundant degrees of freedom movement of the first robot arm sothat for each controllable position and orientation of the working endof the manipulatable device there are a plurality of possibleconfigurations for the first robot arm. The second robot arm includes asecond plurality of joints and a second plurality of links that arecoupled together to provide redundant degrees of freedom movement of thesecond robot arm so that for each controllable position and orientationof the image capturing device there are a plurality of possibleconfigurations for the second robot arm. The controller is configured tocontrol movement of one of the first and second robot arms to beconfigured in one of its plurality of possible configurations to avoid acollision with the other one of the first and second robot arms.

In some examples, the controller is configured to determine which one ofthe first and second robot arms is to be configured in one of itsplurality of possible configurations to avoid a collision with the otherone of the first and second robot arms by processing differences betweencurrent configurations of the first and second robot arms and theirrespective pluralities of possible configurations to minimize a costfunction. In some examples, the controller is configured to determinewhich one of the first and second robot arms is to be configured in oneof its plurality of possible configurations to avoid a collision withthe other one of the first and second robot arms by processing requiredjoint movements of the first and second robotic systems to move fromtheir current configurations to others of their respective pluralitiesof possible configurations to minimize a cost function.

Consistent with some embodiments is a method for avoiding collisionsbetween first and second robot arms. The first robot arm holds amanipulatable device having a working end. The second robot arm holds animage capturing device for capturing a plurality of images of theworking end of the manipulatable device from which a three-dimensionalcomputer model of the working end of the manipulatable device isgeneratable. The method includes determining a position and anorientation of the working end of the manipulatable device relative to areference frame of the image capturing device by using at least oneimage of the plurality of images of the working end of the manipulatabledevice, wherein the reference frame of the image capturing devicecorresponds to a perspective of the captured images; determining aconfiguration and position of one of the first and second robot armsrelative to a common reference frame by using the determined positionand orientation of the working end of the manipulatable device relativeto the reference frame of the image capturing device; determining aconfiguration and position of the other of the first and second robotarms relative to the common reference frame by using joint positioninformation received for the other of the first and second robot arms;determining an imminent collision between the first and second robotarms using the determined configurations and positions of the first andsecond robot arms relative to the common reference frame; and commandingan action to be taken by one of the first and second robot arms to avoidthe imminent collision.

Consistent with some embodiments, a robotic system includes a firstrobot arm that is holding a manipulatable device having a working endand a processor programmed to register the first robot arm to a secondrobot arm that is holding an image capturing device. The processorperforms the registration by performing a low accuracy registration ofthe first and second robot arms at initial positions of the first andsecond robot arms using external tracking data and kinematics data of atleast one of the first and second robot arms; performing a mid accuracyregistration of the first and second robot arms relative to a firstsetup position using at least one image captured by the image capturingdevice of at least a part of the first robot arm and kinematics data ofat least one of the first and second robot arms, wherein the first setupposition includes a first safety margin indicative of the low accuracyregistration; and performing a high accuracy registration of the firstand second robot arms relative to a second setup position using at leastone image captured by the image capturing device of the working end ofthe manipulatable device and kinematics data of at least one of thefirst and second robot arms, wherein the second setup position includesa second safety margin indicative of the mid accuracy registration.

Consistent with some embodiments is a method implemented by a processorfor registering first and second robot arms. The first robot arm holds amanipulatable device having a working end. The second robot arm holds animage capturing device. The method includes performing a low accuracyregistration of the first and second robot arms at initial positions ofthe first and second robot arms using external tracking data andkinematics data of at least one of the first and second robot arms,performing a mid accuracy registration of the first and second robotarms relative to a first setup position using at least one imagecaptured by the image capturing device of at least a part of the firstrobot arm and kinematics data of at least one of the first and secondrobot arms, wherein the first setup position includes a first safetymargin indicative of the low accuracy registration, and performing ahigh accuracy registration of the first and second robot arms relativeto a second setup position using at least one image captured by theimage capturing device of the working end of the manipulatable deviceand kinematics data of at least one of the first and second robot arms,wherein the second setup position includes a second safety marginindicative of the mid accuracy registration.

Additional objects, features and advantages of the various aspects ofthe present invention will become apparent from the followingdescription of its preferred embodiment, whose description should betaken in conjunction with the accompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 illustrates a block diagram of a robotic system utilizing aspectsof the present invention.

FIG. 2 illustrates a flow diagram of a method, implemented in a roboticsystem utilizing aspects of the present invention, for performingmulti-accuracy registration of an image capturing device robot arm and amanipulatable device robot arm as the image capturing device robot armmoves relative to the manipulatable device robot arm.

FIG. 3 illustrates a flow diagram of a method, implemented in a roboticsystem utilizing aspects of the present invention, for automaticallyavoiding a collision between independently operated robot arms from theperspective of an image capturing device reference frame.

FIG. 4 illustrates a flow diagram of the method, implemented in arobotic system utilizing aspects of the present invention, forautomatically avoiding a collision between independently operated robotarms from the perspective of a manipulatable device reference frame.

FIG. 5 illustrates a flow diagram of a method, implemented in a roboticsystem utilizing aspects of the present invention, for controlling animage capturing device robot arm so that its image capturing devicecaptures images of a user selected region of interest while avoiding acollision with another robot arm.

FIG. 6 illustrates a flow diagram of a method, implemented in a roboticsystem utilizing aspects of the present invention, for controllingmovement of an image capturing device robot arm according to a plan ofmotion while avoiding a collision with another robot arm.

FIG. 7 illustrates a block diagram of a portion of a robotic systemutilizing aspects of the present invention, which is used for collisionavoidance of two robot arms operating in a coupled-control mode.

FIG. 8 illustrates a perspective view of an operating room employing amultiple aperture medical robotic system in which aspects of the presentinvention are usable.

FIG. 9 illustrates a front view of a Patient-Side Cart for a multipleaperture medical robotic system in which aspects of the presentinvention are usable.

FIG. 10 illustrates a perspective view of an instrument usable in amultiple aperture medical robotic system in which aspects of the presentinvention are usable.

FIG. 11 illustrates a front view of a console of a robotic systemutilizing aspects of the present invention.

FIG. 12 illustrates a perspective view of an image capturing systemusable in a robotic system utilizing aspects of the present invention.

FIG. 13 illustrates a perspective view of a Patient-Side Cart for asingle aperture medical robotic system in which aspects of the presentinvention are usable.

FIG. 14 illustrates a perspective view of a distal end of an entry guidewith articulated instruments extending out of it as used in a singleaperture medical robotic system in which aspects of the presentinvention are usable.

FIG. 15 illustrates a cross-sectional view of an entry guide as used ina single aperture medical robotic system in which aspects of the presentinvention are usable.

DETAILED DESCRIPTION

FIG. 1 illustrates, as an example, a block diagram of various componentsof a robotic or computer-assisted system 1000 in which methods 2000,2500, and 4000 are implemented for automatically avoiding a collisionbetween two or more robot or movable arms. FIG. 2 describes a multi-stepapproach to registration of robot or movable arms. FIGS. 3, 4 areprovided to describe the methods 2000, 2500 which are generallyapplicable to independently operated robot or movable arms. FIGS. 5-7are provided to describe the method 4000 which is generally applicableto controlling movement of an image capturing device robot or movablearm though a controller which is controlling a manipulatable devicerobot or movable arm. Examples of the robotic system 1000 in which themethods 2000, 2500, and 4000 may be used are described in reference toFIGS. 8-15. FIGS. 8-12 are provided to describe a robotic orcomputer-assisted system 3000 using a Patient-Side Cart 3010 withmultiple robot or movable arms which is suitable for performingprocedures with multiple apertures for entry to a work site. FIGS. 13-15are provided to describe an alternative Patient-Side Cart 4010 for therobotic system 3000, in which the Patient-Side Cart 4010 has a singlerobot or movable arm which is suitable for performing procedures with asingle aperture for entry to a work site.

Before describing details of the methods 2000, 2500, 4000 as illustratedin FIGS. 1-7, the robotic or computer-assisted system 3000 will first bedescribed to provide context and additional details on implementationsof the robotic or computer-assisted system 1000. Although a medicalrobotic or computer-assisted surgical system is described herein as anexample of the robotic or computer-assisted system 1000, it is to beappreciated that the various aspects of the invention as claimed hereinare not to be limited to such types of robotic or computer-assistedsystems.

Referring to FIG. 8, a perspective view of an operating room isillustrated in which a medical robotic system 3000 is provided for aSurgeon to perform a medical procedure on a Patient. The medical roboticsystem in this case is a Minimally Invasive Robotic Surgical (MIRS)system including a Patient-Side Cart 3010, an Image Capturing System3020, and a Console 3030.

The Patient-Side Cart 3010, as described in detail in reference to FIG.9, has a plurality of robot arms for holding and manipulating aplurality of devices such as instruments and at least one endoscope.When using the Patient-Side Cart 3010, each of the devices being held bythe plurality of robot or movable arms is introduced through its ownentry aperture into a Patient.

The Console 3030, as described in detail in reference to FIG. 11,includes input devices for commanding movement of associated ones of theplurality of robot arms of the Patient-Side Cart 3010 and the operationof their respectively held devices. The Console 3030 and thePatient-Side Cart 3010 communicate through a cable 3031.

The Image Capturing System 3020, as described in detail in reference toFIG. 12, captures a plurality of images, such as a series oftwo-dimensional image projections, of one or more objects within aspecified region of interest at a work site in the Patient. Theplurality of images may then be used to generate a three-dimensionalcomputer model of the one or more objects without requiring priorknowledge of the three-dimensional shapes of the one or more objects, ina conventional manner such as used in computed tomography.

A control unit 3021 may be provided beneath or to the side of anarticulated operating table 3040, so that an Assistant may manuallycontrol movement of an image capturing device of the Image CapturingSystem 3020 using a joy stick or other control input provided on thecontrol unit 3021. The control unit 3021 and the Image Capturing System3020 communicate through cable 3022 or using wireless technology.Alternatively, the control unit 3021 may be provided at another locationand communicate with the Image Capturing System 3020 as a wired orwireless mobile entity. The control unit 3021 may include at least oneprocessing unit and memory. In some examples, the processing unit maycontrol operation and/or execution of hardware and/or software incontrol unit 3021. The processing unit may include one or more centralprocessing units (CPUs), multi-core processors, microprocessors,microcontrollers, digital signal processors, field programmable gatearrays (FPGAs), custom processors/application specific integratedcircuits (ASICs), and/or the like. The memory may be used to store oneor more software and/or firmware applications as well as various datastructures to be used by control unit 3021. The memory may also includeone or more types of machine readable media. Some common forms ofmachine readable media may include floppy disk, flexible disk, harddisk, magnetic tape, any other magnetic medium, CD-ROM, any otheroptical medium, punch cards, paper tape, any other physical medium withpatterns of holes, RAM, PROM, EPROM, FLASH-EPROM, any other memory chipor cartridge, and/or any other medium from which a processor or computeris adapted to read.

Alternatively, movement of the image capturing device of the ImageCapturing System 3020 may be controlled by the Surgeon operating anassociated one of the input devices of the Console 3030 according toaspects of the present invention. Alternatively, movement of the imagecapturing device of the Image Capturing System 3020 may be controlledautomatically by a processor of the Console 3030 so that the imagecapturing device automatically captures a plurality of images of a userspecified region of interest in the work site while avoiding a collisionwith the robot arms of the Patient-Side Cart 3010 according to aspectsof the present invention. The Console 3030 and the Image CapturingSystem 3020 communicate through cable 3050.

FIG. 9 illustrates, as an example, a front view of the Patient-Side Cart3010. In a typical application, robot arms 34, 36 hold instruments 33,35 and robot arm 38 holds stereo endoscope 37. The fourth robot arm 32is available so that another instrument 31 may be introduced at the worksite along with the instruments 33, 35 and endoscope 37. Alternatively,the fourth robot arm 32 may be used for introducing a second endoscopeor another image capturing device, such as an ultrasound transducer, tothe work site.

Each of the robot or movable arms is conventionally formed of linkswhich are coupled together and manipulated through actuatable joints.Each of the robot arms includes a setup arm and a device manipulator.The setup arm positions its held device so that a pivot point occurs atits entry aperture into the Patient. The device manipulator may thenmanipulate its held device so that it may be pivoted about the pivotpoint, inserted into and retracted out of the entry aperture, androtated about its shaft axis. In this example, the robot arms aremounted on a movable base 3015 of the Patient-Side Cart 3010.Alternatively, the robot arms may be attached to sliders on a side ofthe operating table, to sliders on a wall or to sliders on the ceilingof the operating room.

FIG. 10 illustrates, as an example, an instrument 100 that may be usedfor either instrument 33, 35 or 31. The instrument 100 comprises aninterface housing 108, a shaft 104, a working end 102, and a wristmechanism 106 which includes one or more wrist joints. The interfacehousing 108 is removably attached to a robot arm so as to bemechanically coupled to actuators (such as motors) in the instrumentmanipulator of the attached robot arm. Cables or rods, that are coupledto the actuators of the instrument manipulator and extend through theshaft 104 from the interface housing 108 to the one or more wrist jointsof the wrist mechanism 106 and to the jaws of the instrument's endeffector 102, actuate the wrist joints and jaws in a conventionalmanner. The instrument manipulator may also manipulate the instrument inpitch and yaw angular rotations about its pivot point at the entryaperture, manipulate the instrument in a roll angular rotation about theinstrument's shaft axis, and insert and retract the instrument along arail on the robot arm as commanded by the processor 43.

FIG. 11 illustrates, as an example, a front view of the Console 3030.The Console 3030 has left and right input devices 41, 42 which the usermay grasp respectively with his/her left and right hands to manipulateassociated devices being held by the plurality of robot or movable armsof the Patient-Side Cart 3010 in preferably six degrees-of-freedom(“DOF”). Foot pedals 44 with toe and heel controls are provided on theConsole 3030 so the user may control movement and/or actuation ofdevices associated with the foot pedals. A processor 43 is provided inthe Console for control and other purposes. A stereo vision display 45is also provided in the Console so that the user may view the work sitein stereo vision from images captured by the stereoscopic camera of theendoscope 37. Left and right eyepieces, 46 and 47, are provided in thestereo vision display 45 so that the user may view left and righttwo-dimensional (“2D”) display screens inside the display 45respectively with the user's left and right eyes.

The processor 43 performs various functions in the medical roboticsystem. One important function that it performs is to translate andtransfer the mechanical motion of input devices 41, 42 to commandactuators in their associated device manipulators to actuate theirrespective joints so that the Surgeon can effectively manipulatedevices, such as the tool instruments 31, 33, 35 and endoscope 37, whichare associated with the input devices 41, 42 at the time. Anotherfunction of the processor 43 is to implement the methods, cross-couplingcontrol logic, and controllers described herein.

Although described as a processor, it is to be appreciated that theprocessor 43 may be implemented by any combination of hardware, softwareand firmware. Also, its functions as described herein may be performedby one unit or divided up among a number of subunits, each of which maybe implemented in turn by any combination of hardware, software andfirmware. Further, although being shown as part of or being physicallyadjacent to the Console, the processor 43 may also be distributed assubunits throughout the system 3000.

U.S. Pat. No. 6,659,939 B2 entitled “Cooperative Minimally InvasiveTelesurgical System,” which is incorporated herein by reference,provides additional details on a multiple aperture medical roboticsystem such as described herein.

FIG. 12 illustrates, as an example, the Image Capturing System 3020which has a robot or movable arm 1101 which is mounted to a base 1106.The robot arm 1101 comprises a carousel 1107, first link 1102, secondlink 1103, wrist 1104, and C-arm 1105. The carousel 1107 is rotatable(as indicted by arrow “a”) relative to the base 1106 using a carouseljoint 1108. The first link 1102 is rotatable (as indicated by arrow “b”)relative to the carousel 1107 using a shoulder joint 1109. The secondlink 1103 is rotatable (as indicated by arrow “c”) relative to the firstlink 1102 using an elbow joint 1110. The wrist 1104 is rotatable (asindicated by arrow “d”) relative to the second link 1103 using a rolljoint 1111. The C-arm 1105 is rotatable (as indicated by arrow “e”)relative to the wrist 1104 using a pitch joint 1112 and rotatable (asindicated by arrow “j”) relative to the wrist 1104 using a yaw joint1121.

The C-arm 1105 comprises a first limb 1113, second limb 1114, and acentral portion 1115. The first and second limbs 1113, 1114 areextendable away from and towards (as indicated by arrow “f”) the centralelement 1115 using an extender joint 1120. The first limb 1113 has anX-ray detector 1116 disposed at its distal end and the second limb 1114has an X-ray source 1117 disposed at its distal end. The X-ray detector1116 is rotatable (as indicated by arrow “g”) relative to the distal endof the first limb 1113. The X-ray source 1117 is rotatable (as indicatedby arrow “h”) relative to the distal end of the second limb 1114. Inthis arrangement, the X-ray source and X-ray detector are disposed onopposing ends of the C-arm 1105 so as to form an image capturing device.By actuating the carousel joint 1108, shoulder joint 1109, elbow joint1110, roll joint 1111, pitch joint 1112, yaw joint 1121, and extenderjoint 1120, the C-arm 1105 may be positioned relative to the Patient sothat the C-arm 1105 may be moved so that the image capturing device(comprising X-ray source 1117 and X-ray detector 1116) may capture aseries of two-dimensional projections of one or more objects within aspecified region of interest at a work site in the Patient. The seriesof two-dimensional projections may then be used to generate athree-dimensional computer model of the one or more objects in aconventional manner for cone beam computed tomography.

Previously incorporated by reference U.S. Published Application2009/0234444 A1 provides additional details on such an Image CapturingSystem and its use during the performance of a medical procedure on apatient.

FIG. 13 illustrates, as an example, an alternative Patient-Side Cart4010 which is usable in the robotic system 3000 to introduce a pluralityof articulated instruments to a work site through a single entryaperture in the Patient by an entry guide 200. The aperture may be aminimally invasive incision or a natural body orifice. The entry guide200 is a cylindrical structure which is held and manipulated by a robotarm 4011, which is mounted on base 4015 and includes a setup arm 4012and an entry guide manipulator 4013. The setup arm 4012 comprises aplurality of links and joints which are used to position the entry guide200 at the aperture. As indicated in the figure, the setup arm 4012includes a prismatic joint for adjusting the height of the setup arm4012 (as indicated by arrow “A”) and a plurality of rotary joints foradjusting the horizontal position of the setup arm 4012 (as indicated byarrows “B” and “C”). The entry guide manipulator 4013 is used torobotically pivot the entry guide 200 (and the articulated instrumentsdisposed within it at the time) in yaw, pitch and roll angular rotationsabout the pivot point as indicated by arrows D, E and F, respectively.Articulated instrument manipulators (not shown) reside in housing 4014.

As shown in FIG. 14, the entry guide 200 has articulated instrumentssuch as articulated surgical instruments 231, 241 and an articulatedstereo camera instrument 211 (or other image capturing deviceinstrument) extending out of its distal end. The camera instrument 211has a pair of stereo image capturing elements 311, 312 and a fiber opticcable 313 (coupled at its proximal end to a light source) housed in itstip. The surgical instruments 231, 241 have working ends 331, 341.Although only two instruments 231, 241 are shown, the entry guide 200may guide additional instruments as required for performing a medicalprocedure at a work site in the Patient. For example, as shown in across-sectional view of the entry guide 200 in FIG. 15, a passage 351 isavailable for extending another articulated surgical tool through theentry guide 200 and out through its distal end. Passages 431, 441, arerespectively used by the articulated surgical tool instruments 231, 241,and passage 321 is used for the articulated camera instrument 211.

When the Patient-Side Cart 4010 is used in the robotic system 3000, thestereo vision display 45 of the Console 3030 displays stereo imagesderived from the stereo images captured by the articulated camerainstrument 211. Also, the processor 43 of the Console 3030 translatesand transfers the mechanical motion of the input devices 41, 42 toactuate joints of devices, such as the entry guide 200, articulatedsurgical instruments 231, 241, and articulated camera instrument 211,which are associated at the time with the input devices 41, 42.

Each of the articulated instruments comprises a plurality of actuatablejoints and a plurality of links coupled to the joints. As an example, asshown in FIG. 14, the second articulated instrument 241 comprises first,second, and third links 322, 324, 326, first and second joints 323, 325,and a wrist assembly 327. The first joint 323 couples the first andsecond links 322, 324 and the second joint 325 couples the second andthird links 324, 326 so that the second link 324 may pivot about thefirst joint 323 in pitch and yaw while the first and third links 322,326 remain parallel to each other. The first articulated instrument 231and the camera articulated instrument 211, may be similarly constructedand operated.

U.S. Pat. No. 7,725,214 entitled “Minimally Invasive Surgical System,”which is incorporated herein by reference, provides additional detailson a single aperture medical robotic system such as described herein.

Now referring back to FIG. 1, a block diagram of components of therobotic system 1000 is illustrated to describe various aspects of thepresent invention. In this example, the robotic system 1000 has a firstmanipulatable device 1001, a first image capturing device 1002, and acamera 1003. The first manipulatable device 1001 may be an instrumentsuch as the one of the instruments 33, 35 held and manipulated by robotarms of the Patient-Side Cart 3010. Alternatively, the firstmanipulatable device 1001 may be an entry guide through whicharticulated instruments extend such as the entry guide 200 held andmanipulated by the robot arm of the Patient-Side Cart 4010. A secondmanipulatable device 1004 is also shown which may be another device suchas the first manipulatable device 1001. A second image capturing device1005 is also shown which may provide a different imaging modality thanthat of the first image capturing device 1002. Although twomanipulatable devices and two image capturing devices are shown forillustrative purposes, it is to be appreciated that in practice, more orless of each of such devices may be included in the robotic system 1000to perform a task or procedure on an object at a work site. Additionalcameras may also be included.

A device controller 1021 controls movement of the robot arm 1011 toposition and orient a working end of the first manipulatable device 1001in response to commands from an input unit 1031. The input unit 1031 maybe a user operated input device, such as one of the input devices 41, 42or the foot pedal 44 of the console 3030. Alternatively, the input unit1031 may be a processor, such as the processor 43 of the console 3030,executing stored program instructions. Alternatively, the input unit1031 may be coupled control logic which communicates through bus 1040with one or more of the controllers 1022, 1023, 1024, 1025 and/or inputunits 1032, 1033, 1034, 1035 associated with the first image capturingdevice 1002, the camera 1003, the second manipulatable device 1004, andthe second image capturing device 1005.

Likewise, a device controller 1024 controls movement of the robot arm1014 to position and orient a working end of the second manipulatabledevice 1004 in response to commands from an input unit 1034. The inputunit 1034 may be a user operated input device, such as one of the inputdevices 41, 42 or the foot pedal 44 of the console 3030. Alternatively,the input unit 1034 may be a processor, such as the processor 43 of theconsole 3030, executing stored program instructions. Alternatively, theinput unit 1034 may be coupled control logic which communicates throughbus 1040 with one or more of the controllers 1021, 1022, 1023, 1025and/or input units 1031, 1032, 1033, 1035 associated with the firstmanipulatable device 1001, the first image capturing device 1002, thecamera 1003, and the second image capturing device 1005.

The first image capturing device 1002 is manipulatable by its robot arm1012 (or other motorized mechanism) to capture a plurality oftwo-dimensional image slices or projections of an object at the worksite from which a three-dimensional model of the object may be computergenerated without prior knowledge of the shape of the object using animaging modality such as ultrasound, X-ray fluoroscopy, ComputedTomography (CT), and Magnetic Resonance Imaging (MRI). A controller 1022controls movement of the robot arm 1012 to position and orient the firstimage capturing device 1002 in response to commands from an input unit1032. The input unit 1032 may be a user operated input device, such asone of the input devices 41, 42 of the console 3030. Alternatively, theinput unit 1032 may be a processor, such as the processor 43 of theconsole 3030, executing stored program instructions. Alternatively, theinput unit 1032 may be coupled control logic which communicates throughbus 1040 with one or more of the controllers 1021, 1023, 1024, 1025and/or input units 1031, 1033, 1034, 1035 associated with the firstmanipulatable device 1001, the camera 1003, the second manipulatabledevice 1004, and the second image capturing device 1005. The first imagecapturing device 1002 and its robot arm 1012 may be combined to form animage capturing system such as the Image Capturing System 3020.

The second image capturing device 1005 may be similarly constructed andoperated as the first image capturing device 1002 to capture a pluralityof two-dimensional image projections of an object at the work site fromwhich a three-dimensional model of the object may be computer generatedwithout prior knowledge of the shape of the object using an imagingmodality such as ultrasound, X-ray fluoroscopy, Computed Tomography(CT), and Magnetic Resonance Imaging (MRI). Typically, different imagingmodalities are provided by the first and second image capturing devices1002, 1005. When similarly constructed as the first image capturingdevice 1002, a controller 1025 controls movement of the robot arm 1015to position and orient the second image capturing device 1005 inresponse to commands from an input unit 1035. The input unit 1035 may bea user operated input device, such as one of the input devices 41, 42 ofthe console 3030. Alternatively, the input unit 1035 may be a processor,such as the processor 43 of the console 3030, executing stored programinstructions. Alternatively, the input unit 1035 may be coupled controllogic which communicates through bus 1040 with one or more of thecontrollers 1021, 1022, 1023, 1024, and/or input units 1031, 1032, 1033,1034, associated with the first manipulatable device 1001, the firstimage capturing device 1002, the camera 1003, and the secondmanipulatable device 1004. The second image capturing device 1005 andits robot arm 1015 may be combined to form an image capturing systemsuch as the Image Capturing System 3020.

Alternatively, the second image capturing device 1005 may be constructeddifferently than that of the first image capturing device 1002. Forexample, the second image capturing device 1005 may be held, positioned,and oriented by one of the first and second manipulatable devices 1001,1004 rather than having its own robot arm, controller, and input unit.Examples of such a second image capturing device include a drop-inultrasound probe or an optical coherent tomography probe. For details ofsuch an ultrasound probe, see, e.g., U.S. 2007/0021738 A1 entitled“Laparoscopic Ultrasound Robotic Surgical System,” which is incorporatedherein by reference.

The camera 1003 may be held and manipulated by a robot arm 1013 tocapture stereoscopic images of work site, such as the endoscope 37 ofFIG. 9. As an alternative example, the camera 1003 may be thearticulated camera instrument 211 of FIG. 14. A controller 1023 controlsmovement of the robot arm 1013 (or joints of the articulated camerainstrument 211) to position and orient an image capturing element of thecamera 1003 in response to commands from an input unit 1033. The inputunit 1033 may be a user operated input device, such as one of the inputdevices 41, 42 of the console 3030. Alternatively, the input unit 1033may be a processor, such as the processor 43 of the console 3030,executing stored program instructions. Alternatively, the input unit1033 may be coupled control logic which communicates through bus 1040with one or more of the controllers 1021, 1022, 1024, 1025 and/or inputunits 1031, 1032, 1034, 1035 associated with the first manipulatabledevice 1001, the image capturing device 1002, the second manipulatabledevice 1004, and the second image capturing device 1005.

Alternatively, the camera 1003 may be held, positioned, and oriented byone of the first and second manipulatable devices 1001, 1004 rather thanhaving its own robot arm, controller, and input unit. For example, thecamera may be a tethered camera which is orientable by a manipulatabledevice pulling on the tether(s). In this case, the camera would not haveits own robot arm, controller, and input unit. An example of such atethered camera is described in U.S. Patent Application Publication No.2012/02900134, entitled “Estimation of a Position and Orientation of aFrame Used in Controlling Movement of a Tool,” which is incorporatedherein by reference.

Each of the robot arms 1011, 1012, 1013, 1014, 1015 includes a pluralityof links and a plurality of actuatable joints whose positions and/orvelocities are sensed by a plurality of joint sensors. Information fromthe plurality of joint sensors of each robot arm 1011, 1012, 1013, 1014,1015 is provided to its respective controller 1021, 1022, 1023, 1024,1025 for control purposes and may be provided to one or more of theother controllers over bus 1040 for collision avoidance purposes, sincethis joint information indicates configuration information for the robotarms 1011, 1012, 1013, 1014, 1015.

Each of the control units 1021-1025 may include at least one processingunit and memory. In some examples, the processing unit may controloperation and/or execution of hardware and/or software in the respectivecontrol unit 1021-1025. Each of the processing units may include one ormore central processing units (CPUs), multi-core processors,microprocessors, microcontrollers, digital signal processors, fieldprogrammable gate arrays (FPGAs), custom processors/application specificintegrated circuits (ASICs), and/or the like. The memory may be used tostore one or more software and/or firmware applications as well asvarious data structures to be used by the respective control unit1021-1025. The memory may also include one or more types of machinereadable media. Some common forms of machine readable media may includefloppy disk, flexible disk, hard disk, magnetic tape, any other magneticmedium, CD-ROM, any other optical medium, punch cards, paper tape, anyother physical medium with patterns of holes, RAM, PROM, EPROM,FLASH-EPROM, any other memory chip or cartridge, and/or any other mediumfrom which a processor or computer is adapted to read.

The robotic system 1000 also includes a stereo viewer or display 1051for displaying stereo images which have been generated by an imageprocessor 1050 from images captured by the stereo camera 1003 and/orthree-dimensional computer models of one or more objects which have beengenerated by the image processor 1050 from the plurality oftwo-dimensional image slices of the one or more objects captured by thefirst image capturing device 1002 and/or second image capturing device1005. Before displaying images derived from both the camera 1003 and oneor both of the first and second image capturing devices 1002, 1005concurrently on the display 1051, the image processor 1050 registers theimages so that the three-dimensional computer models of the one or moreobjects are properly superimposed on and positioned relative to imagesof the one or more objects in the stereo images derived from imagescaptured by the stereo camera 1003.

FIGS. 3, 4 respectively illustrate, as examples, flow diagrams ofmethods 2000, 2500 which may be implemented in the robotic system 1000for avoiding a collision between independently operated image capturingdevice and manipulatable device robot arms. Although shown forexplanatory purposes as two different methods, it is to be appreciatedthat the methods 2000, 2500 are essentially the same method taken fromdifferent perspectives. In particular, the method 2000 is from theperspective of an image capturing device reference frame and may beperformed by an image capturing device controller and the method 2500 isfrom the perspective of a manipulatable device reference frame and maybe performed by a manipulatable device controller.

Although only one manipulatable device is mentioned in describing themethods herein, it is to be appreciated that when the working end ofmore than one device is viewable within the field of view of the imagecapturing device, each of those devices are to be processed as thedescribed manipulatable device according to the methods so thatcollisions may be avoided between their robot arms and the robot arm ofthe image capturing device. For example, when the robotic system 1000 isthe medical robotic system 3000 of FIG. 8 and it includes thePatient-Side Cart 3010 of FIG. 9, the relevant blocks of both methods2000, 2500 are performed for each of the manipulatable devices 31, 33,35, 37 that is viewable in images captured by the image capturing device3020. On the other hand, when the robotic system 1000 is the medicalrobotic system 3000 of FIG. 8 and it includes the Patient-Side Cart 4010of FIG. 13, the relevant blocks of both methods 2000, 2500 are performedfor the entry guide manipulator 4013, but the working ends ofarticulated instruments extending out of the entry guide 200 are treatedas the working end of the entry guide 200.

To avoid confusion and unintended consequences, only one of the methods2000, 2500 is performed to avoid collisions between robot arms. Also,only one of the device controllers preferably performs the collisionavoidance method. The controller taking such action may bepre-established, user selected, or selected by certain criteria.

As an example of user specification, when the robotic system 1000 is themedical robotic system 3000 of FIG. 8 and it includes the Patient-SideCart 3010 of FIG. 9, the Surgeon may interact with a menu beingdisplayed on the stereo viewer 45 to select either the control unit 3021to perform the method 2000 or the processor 43 to perform the method2500. As previously explained, the control unit 3021 controls movementof the robot arm 1101 which holds and manipulates the image capturingdevice 1116, 1117 of the Image Capturing System 3020. The processor 43,on the other hand, implements a controller which controls movement ofthe robot arm 34 that holds and manipulates the instrument 33, which maybe the manipulatable device for the purpose of this example.

When the robotic system 1000 is the medical robotic system 3000 of FIG.8 and it includes the Patient-Side Cart 4010 of FIG. 13, then themanipulatable device referred to in methods 2000, 2500 is the entryguide 200 rather than one of the articulated instruments 211, 231, 241.In this case, it is the entry guide manipulator 4013 that is at risk ofcolliding with or being struck by the robot arm 1101 of the ImageCapturing System 3020. The articulated instruments 211, 231, 241 do nothave significant portions of any robot arms extending outside thePatient's body. They merely have manipulators whose movements aregenerally confined to be within a housing area 4014 of the entry guidemanipulator 4013 as shown in FIG. 13. It is notable that collisionavoidance between the robot arm 1101 of the Image Capturing System 3020and the entry guide manipulator 4013 may be simpler than collisionavoidance between the robot arm 1101 of the Image Capturing System 3020and the robot arms 32, 34, 36, 38 of the Patient-Side Cart 3010. This isbecause not only are there more robot arms 32, 34, 36, 38 of thePatient-Side Cart 3010 for the robot arm 1101 of the Image CapturingSystem 3020 to collide with, but also because the robot arms 32, 34, 36,38 of the Patient-Side Cart 3010 may move more often than the entryguide robot manipulator 4013 of the Patient-Side Cart 4010.

FIG. 2 illustrates, as an example, a flow diagram of a method 9000,which is implemented in the robotic system 1000, for registering animage capturing device robot arm and a manipulatable device robot arm.Using the method, higher accuracy registrations are performed as theimage capturing device robot arm moves closer to the manipulatabledevice robot arm. This multi-step approach provides high accuracyregistration in a safe manner.

In block 9001, the method performs a low accuracy registration (e.g.,within an accuracy range of tens of centimeters) of the image capturingdevice robot arm and the manipulatable device robot arm relative to acommon reference frame at their initial positions using an externaltracker system and kinematic data for the robot arms. As an example, theexternal tracker system may be of the transmitter/receiver type whichconventionally employs transmitters, which are strategically disposed onknown locations of the robot arms of the image capturing device and themanipulatable device, and one or multiple receivers, which are disposedwithin transmission distance of the transmitters. As another example,the external tracker system may be of the optical type whichconventionally employs optically discernible targets, which arestrategically disposed on known locations of the robot arms of the imagecapturing device and the manipulatable device, and one or multipleoptical detectors, which are disposed so as to have an unobstructed viewof the targets. The kinematic data may be provided, for example, byencoders disposed to sense joint positions of the image capturing devicerobot arm and the manipulatable device robot arm. The joint positionsmay then be combined in a conventional manner using knowledge of theconstruction, shapes, and sizes of the robot arms to estimate theconfigurations of the robot arms.

In block 9002, the image capturing device is moved towards themanipulatable device robot arm to assume a low-risk setup position thatis close enough to allow the image capturing device being held by theimage capturing device robot arm to capture images of at least a portionof the manipulatable device robot arm while being sufficiently farenough away from the manipulatable device robot arm to ensure that theimage capturing device robot arm does not collide with the manipulatabledevice robot arm. In determining the low-risk setup position, the methodalso takes into account the current low accuracy level of theregistration through a first safety margin that ensures no collisionwill occur between the image capturing device robot arm and themanipulatable device robot arm. For example, the first safety margin maymaintain a distance of at least ten centimeters between the imagecapturing device robot arm and the manipulatable device robot arm toaccount for the current low accuracy in determining the relativepositions of those two robot arms. The movement of the image capturingdevice robot arm may be performed, for example, by an operatorcommanding such movement through a robot arm controller with assistancefrom the robot arm controller to inhibit unsafe movement. For example,the robot arm controller may provide force feedback to the operatorthrough a haptic device, such as a joystick, to provide resistanceagainst the unsafe movement. Alternatively, it may be performedautomatically by the robot arm controller in either a direct mode orcross-coupled mode.

In block 9003, the method performs a mid accuracy registration (e.g.,within an accuracy range of centimeters) of the image capturing devicerobot arm and the manipulatable device robot arm relative to the commonreference frame at their current positions using one or more capturedimages of at least a part of the manipulatable device robot arm andkinematics data for the robot arms.

In block 9004, the method then waits for an operator's command toinitiate movement of the image capturing device robot arm to captureimages of a user specified region of interest.

In block 9005, the image capturing device is moved towards themanipulatable device robot arm to assume an image capturing setupposition that is close enough to allow the image capturing device beingheld by the image capturing device robot arm to capture images of a userspecified region of interest while being sufficiently far enough awayfrom the manipulatable device robot arm to ensure that the imagecapturing device robot arm does not collide with the manipulatabledevice robot arm. In determining the image capturing setup position, themethod takes into account the current mid accuracy level of theregistration through a second safety margin that ensures no collisionwill occur between the image capturing device robot arm and themanipulatable device robot arm. For example, the second safety marginmay maintain a distance of at least one centimeter between the imagecapturing device robot arm and the manipulatable device robot arm toaccount for the current mid accuracy in determining the relativepositions of those two robot arms. The movement of the image capturingdevice robot arm may be performed, for example, by an operatorcommanding such movement through a robot arm controller with assistancefrom the robot arm controller to inhibit unsafe movement. For example,the robot arm controller may provide force feedback to the operatorthrough a haptic device, such as a joystick, to provide resistanceagainst the unsafe movement. Alternatively, the movement may beperformed automatically by the image capturing device controllerdirectly or through a coupled control mode with the manipulatable devicecontroller.

In block 9006, the method commands the image capturing device robot armto move relative to the region of interest and commands the imagecapturing device being held by the image capturing device robot arm tocapture images of the region of interest during such movement whileavoiding a collision of the image capturing device robot arm and themanipulatable device robot arm. In addition to capturing images of theregion of interest, the image capturing device captures images of atleast a working end of a manipulatable device being held by themanipulatable device robot arm. In this case, the working end of themanipulatable device is proximate to the region of interest so as to bewithin the field of view of the image capturing device. During the imagecapturing process, the method performs at least one high accuracyregistration (e.g., within an accuracy range of millimeters) of theimage capturing device robot arm and the manipulatable device robot armrelative to the common reference frame at their current positions usingone or more of the captured images of the working end of themanipulatable device and kinematics data for the robot arms.

Additional details describing aspects of block 9006 are described belowin reference to FIGS. 3, 4 and additional details describing aspects ofblocks 9005, 9006 are described below in reference to FIG. 5.

Referring now to the method 2000 of FIG. 3, in block 2001, the method,which is preferably performed by the image capturing device controller1022, receives information of the configuration of the robot arm thatholds and manipulates the image capturing device. When the robot armcomprises a plurality of links coupled together by a plurality ofjoints, the robot arm configuration is determinable from sensedpositions of the joints and known geometries of the links and otherstructure making up the robot arm. As previously described in referenceto FIG. 1, such robot arm configuration information may be provided froma plurality of sensors in the image capturing device robot arm, such asencoders which are coupled to actuators of the joints. Alternatively, itmay be joint positions being commanded by the image capturing devicecontroller 1022 in response to commands from the input device 1032.

In block 2002, the method determines whether or not it is time toperform a registration of the image capturing device to themanipulatable device. Such registration may be performed once at thestart of the method or it may be performed periodically to correct anyregistration errors that may accrue over time. If the determination inblock 2002 is YES, then the method performs the registration byperforming blocks 2003, 2004. On the other hand, if the determination inblock 2002 is NO, then registration is skipped by the method jumping toblock 2005.

In block 2003, the method receives an image, which has been captured bythe image capturing device, of the working end of the manipulatabledevice. Generally, the received image will be a two-dimensional slice orprojection of the working end of the manipulatable device and any otherobjects within an image capturing cone of the image capturing device.

In block 2004, the method determines the position and orientation (i.e.,pose) of the working end of the manipulatable device relative to areference frame of the image capturing device, which corresponds to aposition and orientation of the image capturing cone or field of view ofthe image capturing device. To do this, the method conventionally usesartificial and/or natural features of the working end that arediscernible in the received image. In this case, artificial featuresinclude such things as markings or structure specifically placed on theworking end to aid in determining their pose (i.e., position andorientation). Natural features include such things as the shape andknown geometries of structure of the working end of the manipulatabledevice. The pose of the working end may be determinable even though theworking end may be partially occluded. For example, the working end maybe occluded by portions of a patient's anatomy, other portions of themanipulatable device, portions of the image capturing device, othermedical instruments, and/or the like. The patient's anatomy may includesoft tissue, bone, teeth, and/or the like. To aid in the determination,the sequence of images captured by the image capturing device andreceived as the method loops through blocks 2001-2009 may be used torefine the determination of the pose. Also, conventional tool trackingtechniques may be used that aid in and serve to further refine thedetermination of the pose of the working end of the manipulatabledevice. For additional details on such pose determining techniques andartifacts, see U.S. Pat. No. 8,108,072 entitled “Methods and systems forrobotic instrument tool tracking with adaptive fusion of kinematicsinformation and image information,” which is incorporated herein byreference, and U.S. Publication No. 2010/0168763 A1 entitled“Configuration marker design and detection for instrument tracking,”which is incorporated herein by reference.

In block 2005, the method receives information of the configuration ofthe robot arm that holds and manipulates the manipulatable device. Wherethe robot arm comprises a plurality of links coupled together by aplurality of joints, the robot arm configuration is determinable fromsensed positions of the joints and known geometries of the links andother structure making up the robot arm. As previously described inreference to FIG. 1, such robot arm configuration information may beprovided from a plurality of sensors of the manipulatable device robotarm, such as encoders which are coupled to actuators of the joints.Alternatively, it may be joint positions being commanded by themanipulatable device controller 1021 in response to commands from theinput device 1031. Alternatively, it may be some joint positions beingprovided by tracking the working end of instruments in endoscopic imageswhich may provide better accuracy compared to encoder measurements.

If the manipulatable device robot arm is a redundant degree-of-freedom(DOF) arm, then the information of the configuration of the robot armpreferably includes either the sensed joint positions or commanded jointpositions of the manipulatable device robot arm. If, on the other hand,there is only one robot arm configuration that may correspond to thepose of the working end of its held manipulatable device, then the robotarm configuration may theoretically be determined from the determinedpose of the working end of the manipulatable device, the knownconstruction and geometries (e.g., size and shape) of the manipulatabledevice and its robot arm, and the position of the base of themanipulatable device robot arm. In this case, it may not be necessary toreceive information of the sensed or commanded joint positions in block2005, only the base position of the manipulatable device robot arm isnecessary. When the manipulatable device robot arm is mounted to a basethat doesn't move, then the base position is fixed and only needs to bedetermined once and stored in a memory. When the manipulatable devicerobot arm is mounted to a movable base, then the base position may bedetermined, for example, by external sensors such as pressure sensorsstrategically disposed on the floor. As another example, the baseposition may be determined by a transmitter/receiver system in which oneor more transmitters are disposed on the base with one or more receiversdisposed at fixed locations. As another example, the base position (aswell as the robot arm configuration) may be determined by an opticaltracking system or by any other well known position sensing means.

In block 2006, the method determines the configuration and position ofthe manipulatable device robot arm in the image capturing device frameof reference. Since the pose of the working end of the manipulatabledevice relative to the image capturing device reference frame hasalready been determined in block 2004, the determination of theconfiguration and position of its robot arm in the image capturingdevice frame of reference is determined in this case by using the knownconstruction and geometries of the manipulatable device and its robotarm along with the information of the manipulatable device robot armconfiguration received in block 2005.

In block 2007, the method determines the configuration and position ofthe image capturing device robot arm in the image capturing devicereference frame. Since the image capturing device reference frame isdefined by the pose of the distal end of the image capturing devicerobot arm, it is a simple matter to determine the configuration andposition of the image capturing device robot arm in the image capturingdevice reference frame using the known construction and geometries ofthe image capturing device and its robot arm along with the informationof the image capturing device robot arm configuration received in block2001.

In block 2008, the method determines whether an imminent collision isthreatened between the image capturing device robot arm and themanipulatable device robot arm. Preferably, the determination is made byusing the information received or determined in blocks 2001-2007 for thecurrent process cycle and previous process cycles. By using time seriesinformation, not only can the trajectories of the two arms be predicted,but the rates at which they are moving may be estimated. With thisinformation, a collision prediction may be made. When the predictedcollision is within a specified time period, then the collision isconsidered to be imminent, requiring immediate action. The collisionprediction may use a minimum distance between the image capturing devicerobot arm and the manipulatable device robot arm. In determining theminimum, the method preferably approximates links of the robot arms withgeometric shapes that are dimensionally slightly larger than the actuallinks for safety purposes. Since the geometric shapes of the imagecapturing device robot arm and the geometric shapes of the manipulatingdevice robot arm occupy known positions and orientations relative toeach other at this point, it is a straightforward calculation todetermine a minimum distance between the geometric shapes representingthe image capturing device robot arm and the geometric shapesrepresenting the manipulating device robot arm.

One or ordinary skill would also understand that the determination ofblock 2008 may be used to detect additional undesirable relationshipsbetween the manipulating device robot arm and the image capturing devicerobot arm other than an imminent collision. In some embodiments, block2008 may be used to detect when the manipulating device robot arm andthe image capturing device robot arm are in too close a proximity toeach other, even though a collision is not imminent. In someembodiments, block 2008 may be used to detect that the manipulatingdevice robot arm is obstructing a region of interest for which the imagecapturing device is to capture images thereof.

If the determination block 2008 is NO, then the method jumps back toblock 2001 to perform blocks 2001-2008 for a next process cycle. On theother hand, if the determination is YES, then in block 2009, the methodcommands the image capturing device robot arm to take an action to avoida collision with the robot arm of the manipulatable device. Thecommanded action may be to temporarily halt movement of the imagecapturing device robot arm until the manipulatable device robot arm hasmoved to a collision safe position. Alternatively, rather than haltingall movement of the image capturing device robot arm, the speed of itsmovement may be adjusted instead to avoid collision with themanipulatable device robot arm. Alternatively, if the image capturingdevice robot arm is a redundant DOF arm, then an alternative armconfiguration may be commanded to avoid collision with the manipulatabledevice robot arm. Alternatively, the movement of the image capturingdevice robot arm may be halted and the collision avoidance task may bepassed over to the manipulatable device controller to perform the method2500 of FIG. 4.

After performing block 2009, the method then loops back to block 2001 toprocess information for a next process cycle.

Referring now to the method 2500 of FIG. 4, in block 2501, the method,which is preferably performed by the manipulatable device controller1021, receives information of the configuration of the robot arm thatholds and manipulates the manipulatable device. Where the robot armcomprises a plurality of links coupled together by a plurality ofjoints, the robot arm configuration is determinable from sensedpositions of the joints and known geometries of the links and otherstructure making up the robot arm. As previously described in referenceto FIG. 1, such robot arm configuration information may be provided froma plurality of sensors in the image capturing device robot arm, such asencoders which are coupled to actuators of the joints. Alternatively, itmay be joint positions being commanded by the manipulatable devicecontroller 1021 in response to commands from the input device 1031.Alternatively, it may be some joint positions being provided by trackingthe working end of instruments in endoscopic images which may providebetter accuracy compared to encoder measurements.

In block 2502, the method determines whether or not it is time toperform a registration of the image capturing device to themanipulatable device. Such registration may be performed once at thestart of the method or it may be performed periodically to correct anyregistration errors that may accrue over time. If the determination inblock 2502 is YES, then the method performs the registration byperforming blocks 2503, 2504. On the other hand, if the determination inblock 2502 is NO, then registration is skipped by the method jumping toblock 2505.

In block 2503, the method receives an image, which has been captured bythe image capturing device, of the working end of the manipulatabledevice. Generally, the received image will be a two-dimensional slice orprojection of the working end of the manipulatable device and any otherobjects within an image capturing cone or field of view of the imagecapturing device. As an example, the image may be received by themanipulatable device controller 1021 over a bus 1040 (or cable 3050 ofFIG. 8) from the first image capturing device 1002 or a processor in animage capturing system that includes the first image capturing device1002.

In block 2504, the method determines the position and orientation (i.e.,pose) of the working end of the manipulatable device relative to areference frame of the image capturing device in the same manner asdescribed in reference to block 2004 of FIG. 3.

In block 2505, the method receives information of the configuration ofthe robot arm that holds and manipulates the image capturing device.Where the robot arm comprises a plurality of links coupled together by aplurality of joints, the robot arm configuration is determinable fromsensed positions of the joints and known geometries of the links andother structure making up the robot arm. As previously described inreference to FIG. 1, such robot arm configuration information may beprovided from a plurality of sensors of the manipulatable device robotarm, such as encoders which are coupled to actuators of the joints.Alternatively, it may be joint positions being commanded by the imagecapturing device controller 1022 in response to commands from the inputdevice 1032.

In block 2506, the method determines the configuration and position ofthe manipulatable device robot arm in the manipulatable device referenceframe. Since the manipulatable device reference frame is defined by thepose of the distal end of the manipulatable device robot arm, it is asimple matter to determine the configuration and position of themanipulatable device robot arm in the manipulatable device referenceframe using the information received in block 2501 and the knownconstruction and geometries of the manipulatable device and its robotarm.

In block 2507, the method determines the configuration and position ofthe image capturing device robot arm in the manipulatable device frameof reference. One way to do this is to first determine the configurationand position of the image capturing device robot arm in the imagecapturing device reference frame, such as determined in block 2007 ofFIG. 3, then use the following transformation equation to translatepoints of the image capturing device robot arm from the image capturingdevice reference frame to manipulatable device reference frame:

^(M) P= ^(M) _(I) T· ^(I) P  (1)

where ^(M)P is a point in the manipulatable device reference frame “M”,^(M) _(I)T is the image capturing device reference frame “I” tomanipulatable device reference frame “M” transform, and ^(I)P is a pointin the image capturing device reference frame “I”.

The method may determine the transform ^(M) _(I)T by comparing points ofthe working end of the manipulatable device in the image referenceframe, using information of its pose determined in block 2504, withcorresponding points of the working end of the manipulatable device inthe manipulatable device reference frame, using information of its posedetermined from the information of the manipulatable device robot armconfiguration which was received in block 2501 and prior knowninformation of the size, shape, and construction of the manipulatabledevice. For additional details on such reference frame transformations,see previously incorporated by reference U.S. Patent ApplicationPublication No. 2012/02900134, entitled “Estimation of a Position andOrientation of a Frame Used in Controlling Movement of a Tool.”

In block 2508, the method determines whether an imminent collision isthreatened between the image capturing device robot arm and themanipulatable device robot arm in a similar manner as described inreference to block 2008 of FIG. 3. One of ordinary skill would alsounderstand that similar to the determination of block 2008, block 2508may also be used to detect additional undesirable relationships betweenthe manipulating device robot arm and the image capturing device robotarm other than an imminent collision.

If the determination block 2508 is NO, then the method jumps back toblock 2501 to perform blocks 2501-2509 for a next process cycle. On theother hand, if the determination is YES, then in block 2509, the methodmay command either the manipulatable device robot arm or the imagecapturing device robot arm, through the image capturing devicecontroller 1022, to take a collision avoidance action. The entityperforming the collision avoidance action and the particular collisionavoidance action taken may take different forms depending upon systemfactors. One factor is the nature of the tasks being performed at thetime by the image capturing device and the manipulatable device. Anotherfactor is the type or structure of the robot arms or their correspondingvelocity and acceleration at the moment when an imminent collision isdetected.

As an example of the nature of the tasks being performed at the time bythe image capturing device and the manipulatable device, if the roboticsystem 1000 is the medical robotic system 3000 and the manipulatabledevice is one of the instruments 33, 35 which is being used at the timeto perform a delicate surgery on a Patient, then it is desirable not todisturb the robot arm holding the instrument. Therefore, in this case itmay be preferable to take action by modifying the trajectory of theimage capturing device robot arm 1101 to avoid a collision. To avoidsuch collision, movement of the image capturing device robot arm 1101may be temporarily halted until the manipulatable device robot arm hasmoved to a collision safe position. Alternatively, rather than haltingall movement of the image capturing device robot arm 1101, the speedand/or direction of its movement may be adjusted instead to avoidcollision with the manipulatable device robot arm.

As an example of the type or structure of the robot arms, if either orboth of the image capturing device and manipulatable device robot armshas redundant degrees-of-freedom (DOF), then one of the robot arms withsuch redundant DOF may be configured in an alternative configurationwithout affecting the pose of its held device. In particular, if themanipulatable device robot arm has redundant DOF, then it would not benecessary to modify the trajectory of the image capturing device sincethe manipulatable device robot arm may be configured instead to analternative configuration without significantly impacting themanipulation of its held manipulatable device. When both the imagecapturing device and manipulatable device robot arms have redundant DOF,then one of the first and second robot arms is selected for using analternative configuration to avoid the imminent collision by processingdifferences between current configurations of the first and second robotarms and their respective pluralities of possible configurations tominimize a cost function. Alternatively, the selection may be made byprocessing required joint movements of the first and second roboticsystems to move from their current configurations to others of theirrespective pluralities of possible configurations to minimize a costfunction.

Any of several possible cost functions may be minimized by the selectionof an alternative configuration. One possible cost function may be basedon an inverse square of the minimum distance between the first andsecond robot arms. A second possible cost function may be based on aweighted average of an inverse square of distances between links of thefirst and second robot arm, with each link being associated with apassive or active degree of freedom. A third possible cost function maybe based on the second cost function, but may account for only distanceswhich are below a threshold, such as a threshold within an order ofmagnitude of a desired safety margin. A fourth possible cost functionmay be based on a modified version of the second cost function where thedistances are measured between virtual objects or buffer regionssurrounding the links of the first and second robot arms. A fifthpossible cost function may include the buffer regions of the fourth costfunction and the distance threshold of the third cost function. A sixthpossible cost function may be based on a constrained manipulabilityindex for the first and second robot arms with the manipulability indexestimating an ability of the first or second robot arm to move inarbitrary directions around the alternative configuration. A seventhpossible cost function may be based on virtual potential fields appliedto the links and/or joints of the first and second robot arms. As anexample, each link and/or joint of the first and second robot arms mayhave a virtual electric charge assigned to it and the induced virtualrepelling force may be integrated between the first and second robotarms to determine the seventh cost function.

After performing block 2509, the method then loops back to block 2501 toprocess information for a next process cycle.

During or after the performance of methods 2000, 2500, an update orrecalibration of initial registration transforms for work site objectsmay be performed. For example, if the robotic system 1000 is the medicalrobotic system 3000, then it is common practice to register each of thework site objects to a world reference frame. In this case, the worksite objects include the instruments 31, 33, 35, the endoscope 37, theimage capturing device 1116, 1117 of the Image Capturing System 3020,and the Patient's anatomy. During the performance of a procedure,however, registration errors may accumulate or otherwise occur in somemanner. Thus, it may be beneficial to update their initial registrationtransforms with respect to the fixed reference frame using images of theworking ends of the instruments 31, 33, 35 and the endoscope 37, whichhave been captured by the Image Capturing System 3020.

In the methods 2000, 2500 of FIGS. 2, 3, the image capturing device andthe manipulatable device are independently operated typically bydifferent people. For example, when the robotic system 1000 is themedical robotic system 3000, an Assistant standing next to the OperatingTable 3040 may operate the control unit 3021 to control movement of theImage Capturing System 3020 and the Surgeon may operate the inputdevices 41, 42 of the Console 3030 to control movement of themanipulatable devices 33, 35. However, it may be desirable at times forthe Surgeon to also control movement of the Image Capturing System 3020during the performance of a medical procedure. For example, whenendoscopic images of an exterior view of an anatomical structure arebeing displayed on the stereo viewer 45 of the Console 3030, the Surgeonmay want to supplement those images with three-dimensional X-ray imagesof an interior view of the anatomical structure. However, since theSurgeon is occupied with the performance of the medical procedure, amethod for automatically controlling the Image Capturing System 3020 toprovide the Surgeon's desired view of a region of interest is useful.

FIG. 5 illustrates, as an example, a method 4000 implemented in therobotic system 1000 for automatically controlling movement of the imagecapturing device so as to capture images of a user specified region ofinterest while avoiding a collision between its robot arm and anotherrobot arm.

In block 4001, the method receives information of a user specifiedregion of interest at the work site. When the user is viewing the worksite on the display 1051, the region of interest is specified relativeto an image of the work site being displayed at the time on the display1051. The images being displayed on the display 1051 may have beenderived from images captured by a camera 1003 such as a stereoscopicendoscope. Alternatively, they may have been derived from imagescaptured by a second image capturing device 1005 such as an ultrasoundprobe. Alternatively, the images being displayed on the display 1051 mayinclude both images derived from those captured by a camera 1003 andimages derived from those captured by a second image capturing device1005 by registering and superimposing the images.

As an example of one way for the user to specify the region of interestrelative to the image being displayed at the time is to use atelestrator to draw a closed curve around the region of interest on atelestration screen which is displaying the same image being displayedat the time on the display 1051. For details on such a telestrator seeU.S. Published Application No. 2007/0167702 A1 entitled “Medical roboticsystem providing three-dimensional telestration,” which is incorporatedherein by reference. As another example, the user may specify the regionof interest by simply using an input device such as a mouse to controlmovement of a cursor on the display 1051 so that the path of the cursordefines the region of interest. As another example of a way for the userto specify the region of interest, the user may use a gaze tracker and auser controlled switch to indicate when the user's current gaze point onthe display 1051 is to specify a center of a region of interest. In thiscase, the shape of the region of interest may be predefined or it may bedetermined by the shape of an object upon which the user is currentlygazing. As yet another example of a way for the user to specify a regionof interest, the user may telerobotically control movement of a workingend of the manipulatable device so that it touches an object at the worksite to indicate that the region of interest should be the object thathas been touched.

In block 4002, the method receives information of the initial armconfigurations and base positions of the image capturing device robotarm and a manipulatable device robot arm. Information of the initial armconfigurations may be provided by information of their construction andinformation of their joint positions which is received from a pluralityof sensors of the robot arms. Information of the base positions in theworld reference frame may be provided by external sensors which providesuch information. One example of such an external sensor is an opticalsensor that is used in an optical tracking system. Another example is apressure transducer that is one of an array of pressure transducersstrategically placed at known positions on the floor of the work site.

In block 4003, the method determines a set-up position for the imagecapturing device so that it is properly positioned to capture therequested images of the user specified region of interest. In order todo this, the method first translates the region of interest from thedisplay reference frame to an image capturing device reference frameusing a previously determined transform and information received inblock 4002. It then translates the region of interest from the imagecapturing device reference frame to the world reference frame using apreviously determined transform and information received in block 4002.With the region of interest known in the world reference frame, themethod then determines a set-up position from which the image capturingdevice may capture the requested images of the user specified region ofinterest without colliding with the initial position in the worldreference frame of the manipulatable device robot arm.

In block 4004, the method determines a plan of motion for the imagecapturing device to capture the requested images of the user specifiedregion of interest. When the image capturing device is the imagecapturing device 3020 of FIG. 12, the plan of motion may compriseplanned movement of the links 1102, 1103, the wrist 1104 and the C-arm1105.

In block 4005, the method commands movement of the image capturingdevice to its set-up position after receiving a command to do so fromthe user. When the image capturing device is the image capturing device1116, 1117 of the Image Capturing System 3020 of FIG. 12, the commandedmovement typically entails rotating the carousel joint 1108 so that therobot arm 1101 is facing the region of interest and rotating theshoulder joint 1109, elbow joint 1110, roll joint 1111, pitch joint1112, and yaw joint 1121 so that the X-ray source 1117 and X-raydetector 1116 (i.e., the image capturing device) are properly positionedto start capturing the requested images of the user specified region ofinterest.

In block 4006, the method commands incremental movement of the imagecapturing device in accordance with the plan of motion while avoiding acollision between the image capturing device robot arm and themanipulatable device robot arm. Additional details of tasks performed bythe method in block 4006 are described in reference to FIG. 6.

In block 4007, the method determines whether the plan of motion iscompleted. If the determination in block 4007 is NO, then the methodjumps back to block 4006 to command another incremental movement of theimage capturing device according to the plan of motion while avoiding acollision between the image capturing device robot arm and themanipulatable device robot arm.

On the other hand, if the determination in block 4007 is YES, then inblock 4008, the method commands the image capturing device to move backto its initial position or a predefined parking position upon receivinga command to do so from the user, so that it is out of the way andpresents no further risk of collision with the manipulatable devicerobot arm.

FIG. 6 illustrates, as an example, a flow diagram of a method forperforming block 4006 of the method 4000. To supplement the description,FIG. 7 illustrates a block diagram of a portion of a robotic systemwhich is used for collision avoidance between two independently operatedrobot arms. In the example illustrated by FIG. 7, a manipulatable devicerobot arm simulator 6003, an imminent collision detector 6021, an imagecapturing device robot arm simulator 6013, and an incremental motiongenerator 6004 are preferably implemented in a conventional manner by aprocessor executing program code.

In block 4201, the method receives a movement command for themanipulatable device robot arm. For example, referring to FIG. 7, theoutput of the manipulatable device controller 6011 provides jointcommands for the manipulatable device robot arm actuators 6012.

In block 4202, the method determines the position and configuration forthe manipulatable device robot arm. For example, referring to FIG. 7,the manipulatable device robot arm simulator unit 6003 generatesinformation of the position and configuration for the manipulatabledevice robot arm using the joint commands from the manipulatable devicecontroller 6011 and prior knowledge of the construction of themanipulatable device robot arm.

In block 4203, the method determines a desired incremental movement ofthe image capturing device. For example, referring to FIG. 7, theincremental motion generator 6004 generates a desired incrementalmovement of the image capturing device using information of the plan ofmotion determined in block 4004 of FIG. 5 and stored in memory 6005. Thedesired incremental movement in this case depends upon a desiredvelocity for the movement of the image capturing device and a processcycle period for looping through blocks 4006-4007 of FIG. 5.

In block 4204, the method determines the position and configuration forthe image capturing device robot arm. For example, referring to FIG. 7,the image capturing device robot arm simulator unit 6013 generatesinformation of the position and configuration for the image capturingdevice robot arm using the desired incremental motion commands providedby the incremental motion generator 6004, knowledge of the currentposition and configuration of the image capturing device robot arm, andprior knowledge of the construction of the image capturing device robotarm.

In block 4205, the method determines whether an imminent collision isthreatened between the image capturing device robot arm and themanipulatable device robot arm in a similar manner as performed in block2008 of FIG. 3. For example, referring to FIG. 7, the imminent collisiondetector 6021 determines whether a collision between the two arms isimminent by using the information provided by the robot arm simulators6003, 6013 for the current process cycle and previous process cycles. Byusing time series information, not only can the trajectories of the tworobot arms be predicted, but the rates at which they are moving may beestimated. With this information, a collision prediction may be made.When the predicted collision is within a specified time period, then thecollision is considered to be imminent, requiring immediate action.

If the determination in block 4205 is NO, then in block 4206, the methodcommands the image capturing device robot arm to move according to thedesired incremental movement. On the other hand, if the determination inblock 4205 is YES, then in block 4207, the method modifies theincremental movement command to avoid the imminent collision, using asexamples, one of the actions described in reference to block 2009 ofFIG. 3. For example, after the imminent collision detector 6021 hasdetermined that a collision between the image capturing device robot armand the manipulatable device robot arm is imminent, it sends anindication of such to the manipulatable device controller 6011, which inturn, relays the indication to the image capturing device controller6001 so that it can take corrective action to avoid the collision.

Some embodiments of the control units 1021-1025 and/or 3021 may includenon-transient, tangible, machine readable media that include executablecode that when run by one or more processors may cause the one or moreprocessors (e.g., the processing units of control units 1021-1025 and/or3021) to perform the processes of methods 2000, 2500, 4000, and/or 9000as described above. Some common forms of machine readable media that mayinclude the processes of methods 2000, 2500, 4000, and/or 9000 are, forexample, floppy disk, flexible disk, hard disk, magnetic tape, any othermagnetic medium, CD-ROM, any other optical medium, punch cards, papertape, any other physical medium with patterns of holes, RAM, PROM,EPROM, FLASH-EPROM, any other memory chip or cartridge, and/or any othermedium from which a processor or computer is adapted to read.

Although the various aspects of the present invention have beendescribed with respect to a preferred embodiment, it will be understoodthat the invention is entitled to full protection within the full scopeof the appended claims.

What is claimed is:
 1. A movement control system comprising: a trackingsystem; and a controller coupled to the tracking system, the controllercomprising: one or more processors, and memory coupled to the one ormore processors, wherein the movement control system is configured tocouple to: a computer-assisted device having a first movable armconfigured to couple to a manipulatable device having a working end, anda second movable arm separate from the computer-assisted device, thesecond movable arm configured to couple to an image capturing device,and wherein the controller is configured to: determine, based on atleast first tracking data from the tracking system, a first pose of thefirst movable arm; determine, based on at least second tracking datafrom the tracking system, a second pose of the second movable arm; senda first movement command to the second movable arm that directs thesecond movable arm to move into a third pose while maintaining a firstsafety margin between the first movable arm and the second movable arm,wherein in the third pose the image capturing device can capture animage of at least a portion of the manipulatable device, and wherein thefirst movement command is based on the first pose and the second pose;and receive one or more first images from the image capturing device,the one or more first images capturing the at least the portion of themanipulatable device.
 2. The movement control system of claim 1, whereinthe controller is further configured to: determine, based on at leastthe one or more first images, a fourth pose of the first movable arm;determine, based on at least the one or more first images, a fifth poseof the second movable arm; send a second movement command to the secondmovable arm that directs the second movable arm to move into a sixthpose while maintaining a second safety margin between the first movablearm and the second movable arm, wherein in the sixth pose the imagecapturing device can capture an image of the working end, wherein thesixth pose differs from the third pose, and wherein the second movementcommand is based on the fourth pose and the fifth pose; and receive oneor more second images from the image capturing device, the one or moresecond images capturing the working end.
 3. The movement control systemof claim 2, wherein the second safety margin is smaller than the firstsafety margin.
 4. The movement control system of claim 2, wherein thecontroller is further configured to: determine, based on at least theone or more second images, a seventh pose of the first movable arm;determine, based on at least the one or more second images, an eighthpose of the second movable arm; and send a third movement command to thesecond movable arm that directs the second movable arm to move into aninth pose while maintaining a third safety margin between the firstmovable arm and the second movable arm, wherein in the ninth pose theimage capturing device can capture an image of a region of interest,wherein the ninth pose is different from the sixth pose, and wherein thesecond movement command is based on the seventh pose and the eighthpose.
 5. The movement control system of claim 4, wherein the thirdsafety margin is smaller than the second safety margin.
 6. The movementcontrol system of claim 1, wherein the controller is further configuredto: determine, based on at least one or more second images of theworking end, a fourth pose of the first movable arm; determine, based onat least the one or more first images, a fifth pose of the secondmovable arm; and send a second movement command to the second movablearm that directs the second movable arm to move into a sixth pose whilemaintaining a second safety margin between the first movable arm and thesecond movable arm, wherein in the sixth pose the image capturing devicecan capture an image of a region of interest, wherein the sixth pose isdifferent from the third pose, and wherein the second movement commandis based on the fourth pose and the fifth pose.
 7. The movement controlsystem of claim 1, further comprising: a viewer adapted to display oneor more second images of a work space of the working end of themanipulatable device; and an input unit configured to receiveinformation of a user specified region of interest within the work spacedepicted by the one or more second images being displayed on the viewer.8. The movement control system of claim 1, wherein the controller isfurther configured to: receive first kinematic data for the firstmovable arm; receive second kinematic data for the second movable arm;determine the first pose of the first movable arm further based on thefirst kinematic data; and determine the second pose of the secondmovable arm further based on the second kinematic data.
 9. The movementcontrol system of claim 1, wherein: the second movable arm has redundantdegrees of freedom so that for each controllable pose of the imagecapturing device there are a first plurality of possible positions andorientations for the second movable arm; and the first movement commandsent to the second movable arm directs the second movable arm to move toone of the first plurality of possible positions and orientations so asto minimize a cost function.
 10. The movement control system of claim 1,wherein the computer-assisted device is a computer-assisted medicaldevice.
 11. A method of controlling movement, the method comprising:determining, based on at least first tracking data from a trackingsystem, a first pose of a first movable arm of a computer-assisteddevice, the first movable arm configured to move a manipulatable devicehaving a working end; determining, based on at least second trackingdata from the tracking system, a second pose of a second movable arm ofan image capturing device separate from the computer-assisted device;sending a first movement command to the second movable arm that directsthe second movable arm to move into a third pose while maintaining afirst safety margin between the first movable arm and the second movablearm, wherein in the third pose the image capturing device can capture animage of at least a portion of the manipulatable device, and wherein thefirst movement command is based on the first pose and the second pose;and receiving one or more first images from the image capturing device,the one or more first images capturing the at least the portion of themanipulatable device.
 12. The method of claim 11, further comprising:determining, based on at least the one or more first images, a fourthpose of the first movable arm; determining, based on at least the one ormore first images, a fifth pose of the second movable arm; sending asecond movement command to the second movable arm that directs thesecond movable arm to move into a sixth pose while maintaining a secondsafety margin between the first movable arm and the second movable arm,wherein in the sixth pose the image capturing device can capture animage of the working end, wherein the sixth pose differs from the thirdpose, and wherein the second movement command is based on the fourthpose and the fifth pose; and receiving one or more second images fromthe image capturing device, the one or more second images capturing theworking end.
 13. The method of claim 12, wherein the second safetymargin is smaller than the first safety margin.
 14. The method of claim12, further comprising: determining, based on at least the one or moresecond images, a seventh pose of the first movable arm; determining,based on at least the one or more second images, an eighth pose of thesecond movable arm; and sending a third movement command to the secondmovable arm that directs the second movable arm to move into a ninthpose while maintaining a third safety margin between the first movablearm and the second movable arm, wherein in the ninth pose the imagecapturing device can capture an image of a region of interest, whereinthe ninth pose is different from the sixth pose, and wherein the secondmovement command is based on the seventh pose and the eighth pose. 15.The method of claim 11, further comprising: determining, based on atleast one or more second images of the working end, a fourth pose of thefirst movable arm; determining, based on at least the one or more firstimages, a fifth pose of the second movable arm; and sending a secondmovement command to the second movable arm that directs the secondmovable arm to move into a sixth pose while maintaining a second safetymargin between the first movable arm and the second movable arm, whereinin the sixth pose the image capturing device can capture an image of aregion of interest, wherein the sixth pose is different from the thirdpose, and wherein the second movement command is based on the fourthpose and the fifth pose.
 16. The method of claim 11, further comprising:receiving first kinematic data for the first movable arm; receivingsecond kinematic data for the second movable arm; determining the firstpose of the first movable arm further based on the first kinematic data;and determining the second pose of the second movable arm further basedon the second kinematic data.
 17. The method of claim 11, wherein: thesecond movable arm has redundant degrees of freedom so that for eachcontrollable pose of the image capturing device there are a firstplurality of possible positions and orientations for the second movablearm; and the first movement command sent to the second movable armdirects the second movable arm to move to one of the first plurality ofpossible positions and orientations so as to minimize a cost function.18. The method of claim 11, wherein the computer-assisted device is acomputer-assisted medical device.
 19. A non-transitory machine-readablemedium comprising a plurality of machine-readable instructions whichwhen executed by one or more processors are adapted to cause the one ormore processors to perform a method comprising; determining, based on atleast first tracking data from a tracking system, a first pose of afirst movable arm of a computer-assisted device, the first movable armconfigured to move a manipulatable device having a working end;determining, based on at least second tracking data from the trackingsystem, a second pose of a second movable arm of an image capturingdevice separate from the computer-assisted device; sending a firstmovement command to the second movable arm that directs the secondmovable arm to move into a third pose while maintaining a first safetymargin between the first movable arm and the second movable arm, whereinin the third pose the image capturing device can capture an image of atleast a portion of the manipulatable device, and wherein the firstmovement command is based on the first pose and the second pose; andreceiving one or more first images from the image capturing device, theone or more first images capturing the at least the portion of themanipulatable device.
 20. The non-transitory machine-readable medium ofclaim 19, wherein the method further comprises: determining, based on atleast the one or more first images, a fourth pose of the first movablearm; determining, based on at least the one or more first images, afifth pose of the second movable arm; sending a second movement commandto the second movable arm that directs the second movable arm to moveinto a sixth pose while maintaining a second safety margin between thefirst movable arm and the second movable arm, wherein in the sixth posethe image capturing device can capture an image of the working end,wherein the sixth pose differs from the third pose, and wherein thesecond movement command is based on the fourth pose and the fifth pose;and receiving one or more second images from the image capturing device,the one or more second images capturing the working end; wherein thesecond safety margin is smaller than the first safety margin.
 21. Thenon-transitory machine-readable medium of claim 20, wherein the methodfurther comprises: determining, based on at least the one or more secondimages, a seventh pose of the first movable arm; determining, based onat least the one or more second images, an eighth pose of the secondmovable arm; and sending a third movement command to the second movablearm that directs the second movable arm to move into a ninth pose whilemaintaining a third safety margin between the first movable arm and thesecond movable arm, wherein in the ninth pose the image capturing devicecan capture an image of a region of interest, wherein the ninth pose isdifferent from the sixth pose, and wherein the second movement commandis based on the seventh pose and the eighth pose.